#include <tr1/memory>

#include "Dynamics.hpp"
#include "BandedSystem.hpp"

Dynamics::Dynamics() : crNicParametersChanged(true) {
}

void Dynamics::setStages(size_t nStages, size_t nPhiStage){
	density.clear(nPhiStage*nStages, 0, nStages);
	stageParams.resize(nStages);
	time=0;
	crNicParametersChanged = true;
}

void Dynamics::setStageParams(std::vector<StageParam>& stageParams) {
	if (this->stageParams.size() != stageParams.size()) {
		std::cerr << "Error in dynamics::setStageParams\n";
		exit(1);
	}
	this->stageParams = stageParams;
	crNicParametersChanged = true;
}


double Dynamics::oneStepUpWind(double alpha0) {
	double maxDelT = 0;
	std::vector<double> nu(getNStages()), k(getNStages());
	for (size_t i = 0; i != getNStages(); i++) {
		nu[i]=2*stageParams[i].getV();
		k[i]=nu[i]*nu[i]/4/stageParams[i].getD();
		double maxDelTi = 2 / (nu[i] / density.getDX() + k[i]);
		if (maxDelTi > maxDelT)
			maxDelT = maxDelTi;
	}
	/*if (dT < 0)
		dT = maxDelT;*/
	if (dT > maxDelT) {
		std::cerr << "Delta t should be less than " << maxDelT << std::endl;
		exit(0);
	}
	for (size_t i = 0; i != getNStages(); i++) {
		double ki = k[i];
		double nukdp = nu[i] / ki / density.getDX();
		double a00 = 2 / ki / dT + 1;
		register double a11 = 2 / ki / dT + nukdp + 1;
		register double aa1 = 1/(a00 * a11 - 1);
		a00 *= aa1;
		a11 *= aa1;
		register double b00 = 2 / ki / dT - 1;
		register double b11 = 2 / ki / dT - nukdp - 1;
		double alphaDens = 2 / nu[i];

		double p1left = alpha0 * alphaDens;

		for (ProbDens<nVStates>::iterator p0 = density.iter(0,i+1e-30), p1 =
					 density.iter(1,i+1e-30), pend = density.iter(0,i+1+1e-30); p0 != pend; p0++, p1++) {
			double b0 = b00 * (*p0) + *p1;
			double b1 = *p0 + b11 * (*p1) + nukdp * p1left;
			p1left = *p1;
			*p0 = a11 * b0 + aa1*b1;
			p1left += (*p1 = a00 * b1 + aa1*b0);
		}
		alpha0 = p1left / alphaDens;
	}
	time += dT;
	return alpha0;
}

double Dynamics::oneStepCrNic(double alpha0) {
	size_t N2 = 2*density.getNX()/getNStages();
	double dX = density.getDX();
	double maxDelT = 0;
	std::vector<double> nu(getNStages()), k(getNStages());
	for (size_t i = 0; i != getNStages(); i++) {
		nu[i]=2*stageParams[i].getV();
		k[i]=nu[i]*nu[i]/4/stageParams[i].getD();
		double maxDelTi = 2.0/(nu[i]/dX + k[i]);
		if (maxDelTi > maxDelT)
			maxDelT = maxDelTi;
	}

	if (dT > maxDelT) {
		std::cerr << "Delta t should be less than " << maxDelT << std::endl;
		exit(0);
	}

	if (bandedSystems.size() != getNStages()) {
		bandedSystems.clear();
		for (size_t i = 0; i != getNStages(); i++) {
			bandedSystems.push_back(std::tr1::shared_ptr<BandedSystem>(new BandedSystem(N2, 3, 3)));
		}
		crNicParametersChanged = true;
	}
	if (crNicParametersChanged) {
		for (size_t i = 0; i != getNStages(); i++) {
			BandedSystem& bs = *bandedSystems[i];
			double a02=2.0/k[i]/dT + 1;
			double a11=-nu[i]/2/k[i]/dX;
		    double a13=2.0/k[i]/dT + 1;
		    double a15=nu[i]/2/k[i]/dX;
			bs.elementOfA(0, 0) = a02;
			bs.elementOfA(0, 1) = -1.0;
			bs.elementOfA(1, 0) = -1.0;
			bs.elementOfA(1, 1) = a13;
			bs.elementOfA(1, 3) = a15;
			size_t j2;
			for (j2 = 2; j2 < N2-2; j2+=2) {
				bs.elementOfA(j2, j2) = a02;
				bs.elementOfA(j2, j2+1) = -1.0;
				bs.elementOfA(j2+1, j2-1) = a11;
				bs.elementOfA(j2+1, j2) = -1.0;
				bs.elementOfA(j2+1, j2+1) = a13;
				bs.elementOfA(j2+1, j2+3) = a15;
			}
			bs.elementOfA(j2, j2) = a02;
			bs.elementOfA(j2, j2+1) = -1.0;
			bs.elementOfA(j2+1, j2-1) = a11;
			bs.elementOfA(j2+1, j2) = -1.0;
			//Rever  a definição matemática
			bs.elementOfA(j2+1, j2+1) = a13 + 2*a15;
			bs.LUDecompose();
		}
		crNicParametersChanged = false;
	}

	for (size_t i = 0; i != getNStages(); i++) {
		BandedSystem& bs = *bandedSystems[i];
		double b02 = 2.0/k[i]/dT - 1;
		double b11 = nu[i]/2.0/k[i]/dX;
		double b13 = 2.0/k[i]/dT - 1;
		double b15 = - nu[i]/2.0/k[i]/dX;
		ProbDens<nVStates>::iterator p2 = density.iter(0, i+dX/2);
		ProbDens<nVStates>::iterator p3 = density.iter(1, i+dX/2);
		ProbDens<nVStates>::iterator p5 = p3 + 1;
		bs.elementOfB(0) = b02*(*p2) + *p3;
		bs.elementOfB(1) = *p2 + b13*(*p3) + b15*(*p5) + 1.0/(k[i]*dX)*alpha0;
		ProbDens<nVStates>::iterator p1 = p3;
		++p2;
		++p3;
		++p5;
		size_t j2;
		for (j2 = 2; j2 < N2-2; j2+=2) {
			bs.elementOfB(j2) = b02*(*p2) + *p3;
			bs.elementOfB(j2+1) = b11*(*p1) + *p2 + b13*(*p3) + b15*(*p5);
			++p1;
			++p2;
			++p3;
			++p5;
		}
		bs.elementOfB(j2) = b02*(*p2) + *p3;
		bs.elementOfB(j2+1) = 2*b11*(*p1) + *p2 + (b13+2*b15)*(*p3);

		double p1_old = *p1;
		double p3_old = *p3;

		bs.solveLU();

		ProbDens<nVStates>::iterator pp0 = density.iter(0, i+dX/2);
		ProbDens<nVStates>::iterator pp1 = density.iter(1, i+dX/2);
		for (size_t j2 = 0; j2 < N2; j2+=2) {
			*(pp0++) = bs.elementOfB(j2);
			*(pp1++) = bs.elementOfB(j2+1);
		}

	alpha0 = nu[i]/4.0*(3*(p3_old + *p3) - (p1_old + *p1));

	}

	time += dT;
	return alpha0;
}

void CompletionTime(ProbDens<1>& probDens); 
